#include "board_info.h"

BoardInfo::BoardInfo() : i2c_(0), mpu6050_(i2c_) {
  aht = new Adafruit_AHTX0();
  ina219 = new Adafruit_INA219(0x41);
}

BoardInfo::~BoardInfo() {
  if (aht)
    delete aht;
  if (ina219)
    delete ina219;
}

bool BoardInfo::Start() {
  i2c_.setPins(18, 8);
  i2c_.begin();

  if (!ina219->begin(&i2c_)) {
    Serial.println("Failed to find INA219 chip");
    delete ina219;
    ina219 = nullptr;
  } else {
    Serial.println("Found ina219");
  }
  if (aht->begin(&i2c_)) {
    Serial.println("Found AHT20");
  } else {
    Serial.println("Didn't find AHT20");
    delete aht;
    aht = nullptr;
  }
  mpu6050_.begin();
  mpu6050_.calcGyroOffsets(true);
  return true;
}

BoadrInfoData BoardInfo::Get() {
  BoadrInfoData data;
  sensors_event_t humidity, temp;
  if (aht) {
    aht->getEvent(&humidity, &temp);
    data.temperature = temp.temperature;
    data.humidity = humidity.relative_humidity;
  }
  if (ina219) {
    float shuntvoltage = ina219->getShuntVoltage_mV();
    float busvoltage = ina219->getBusVoltage_V();
    float current_mA = ina219->getCurrent_mA();

    // Compute load voltage, power, and milliamp-hours.
    float loadvoltage = busvoltage + (shuntvoltage / 1000);
    float power_mW = loadvoltage * current_mA;
    (void)power_mW;

    total_mA += current_mA;
    total_sec += 1;
    float total_mAH = total_mA / 3600.0;
    (void)total_mAH;

    data.current_mA = current_mA;
    data.loadvoltage = loadvoltage;
    data.power_mW = power_mW;
    data.total_mAH = total_mAH;
  }

  data.mpu_data.temp = mpu6050_.getTemp();
  data.mpu_data.acc_x = mpu6050_.getAccX();
  data.mpu_data.acc_y = mpu6050_.getAccY();
  data.mpu_data.acc_z = mpu6050_.getAccZ();
  data.mpu_data.gyro_x = mpu6050_.getGyroX();
  data.mpu_data.gyro_y = mpu6050_.getGyroY();
  data.mpu_data.gyro_z = mpu6050_.getGyroZ();
  data.mpu_data.acc_angle_x = mpu6050_.getAccAngleX();
  data.mpu_data.acc_angle_y = mpu6050_.getAccAngleY();
  data.mpu_data.gyro_angle_x = mpu6050_.getGyroAngleX();
  data.mpu_data.gyro_angle_y = mpu6050_.getGyroAngleY();
  data.mpu_data.gyro_angle_z = mpu6050_.getGyroAngleZ();
  data.mpu_data.angle_x = mpu6050_.getAngleX();
  data.mpu_data.angle_y = mpu6050_.getAngleY();
  data.mpu_data.angle_z = mpu6050_.getAngleZ();

  return data;
}

void BoardInfo::print(const BoadrInfoData& data) {
  Serial.printf("loadvoltage, %.2f V \r\n", data.loadvoltage);
  Serial.printf("current_mA, %.2f mA\r\n", data.current_mA);
  Serial.printf("Temp: %f C, HUM: %f % \r\n", data.temperature, data.humidity);

  Serial.printf("temp: %.2f\r\n", data.mpu_data.temp);
  Serial.printf("acc: [x:%.2f, y:%.2f, z:%.2f]\r\n", data.mpu_data.acc_x,
                data.mpu_data.acc_y, data.mpu_data.acc_z);
  Serial.printf("gyro: [x:%.2f, y:%.2f, z:%.2f]\r\n", data.mpu_data.gyro_x,
                data.mpu_data.gyro_y, data.mpu_data.gyro_z);
  Serial.printf("gyro angle: [x:%.2f, y:%.2f, z:%.2f]\r\n",
                data.mpu_data.gyro_angle_x, data.mpu_data.gyro_angle_y,
                data.mpu_data.gyro_angle_z);
  Serial.printf("angle: [x:%.2f, y:%.2f, z:%.2f]\r\n", data.mpu_data.angle_x,
                data.mpu_data.angle_y, data.mpu_data.angle_z);
  Serial.printf("---------------------------------------\r\n");
}

void BoardInfo::UpdateMPU() {
  mpu6050_.update();
}
